
/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-11 18:13:36
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-04-11 15:16:16
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\attitude\attitude.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-09-11     UAVI02       the first version
 */
#include "fc_sensors.h"
#include "pid.h"
#include "attitude.h"
#include "imu.h"
#include "fc_error.h"

#define DBG_TAG "attitude"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

// pid_obj_t pidData[PID_ANGLE_ROLL];
// pid_obj_t pidData[PID_ANGLE_PITCH];
// pid_obj_t pidData[PID_ANGLE_YAW];

// pid_obj_t pidData[PID_RATE_ROLL];
// pid_obj_t pidData[PID_RATE_PITCH];
// pid_obj_t pidData[PID_RATE_YAW];


static float originKp[3];
void attitudePidInit(float dt)
{
    pid_Reinit_all();
    // pidInit(&pidData[PID_ANGLE_ROLL], &pidConfig()->pid[PID_ANGLE_ROLL], dt);
    // pidInit(&pidData[PID_ANGLE_PITCH], &pidConfig()->pid[PID_ANGLE_PITCH], dt);
    // pidInit(&pidData[PID_ANGLE_YAW], &pidConfig()->pid[PID_ANGLE_YAW], dt);

    // pidInit(&pidData[PID_RATE_ROLL], &pidConfig()->pid[PID_RATE_ROLL], dt);
    // pidInit(&pidData[PID_RATE_PITCH], &pidConfig()->pid[PID_RATE_PITCH], dt);
    // pidInit(&pidData[PID_RATE_YAW], &pidConfig()->pid[PID_RATE_YAW], dt);

    originKp[PID_RATE_ROLL] = pidData[PID_RATE_ROLL].kp;
    originKp[PID_RATE_PITCH] = pidData[PID_RATE_PITCH].kp;
    originKp[PID_RATE_YAW] = pidData[PID_RATE_YAW].kp;
}
extern float getThrustLpf();
/**
 * @brief Get the Throttle Tpa object
 *
 * @param throttle 当前油门
 * @param breakPoint 不稳定时的油门
 * @param maxThrottle 电机最大油门
 * @param minTpa 最小输出多少
 * @return float [1,minTpa]
 */
float getThrottleTpa(float throttle, float breakPoint, float maxThrottle, float minTpa)
{
    float throttleTpa = 1.f;
    // NOTE 根据油门线性补偿
    if (throttle > breakPoint)
    {
        float percent = 1.f - (throttle - breakPoint) / (maxThrottle - breakPoint); // 1~0
        throttleTpa = minTpa + (1.f - minTpa) * percent;                            //
    }

    return throttleTpa;
}
void attitudeRatePID(attitude_t *Re, fc_control_t *out, zdrone_state_t *state)
{
    // pidData[PID_RATE_ROLL].kp = originKp[PID_RATE_ROLL] * getThrottleTpa(getThrustLpf(), 700, 850, 0.5f);
    // pidData[PID_RATE_PITCH].kp =originKp[PID_RATE_PITCH] * getThrottleTpa(getThrustLpf(), 700, 850, 0.5f);
    // pidData[PID_RATE_YAW].kp = originKp[PID_RATE_YAW] * getThrottleTpa(getThrustLpf(), 700, 900, 0.5f);
    out->roll = pidUpdate(&pidData[PID_RATE_ROLL], Re->roll - state->attitudeRate.roll);
    out->pitch = pidUpdate(&pidData[PID_RATE_PITCH], Re->pitch - state->attitudeRate.pitch);
    Re->yaw = constrainf(Re->yaw, -200.0f, 200.0f);
    out->yaw = pidUpdate_yaw(&pidData[PID_RATE_YAW], Re->yaw - state->attitudeRate.yaw, 0.8f);
    // control->yaw = pidUpdate(&pidData[PID_RATE_YAW], attitudeRateExpect->yaw - state->attitudeRate.yaw);
}

void attitudeAnglePID(attitude_t *Ae, zdrone_state_t *state, attitude_t *Re)
{
    Re->roll = pidUpdate(&pidData[PID_ANGLE_ROLL], Ae->roll - state->attitude.roll);
    Re->pitch = pidUpdate(&pidData[PID_ANGLE_PITCH], Ae->pitch - state->attitude.pitch);
    float yawError = Ae->yaw - state->attitude.yaw;
    if (yawError > 180.0f)
        yawError -= 360.0f;
    else if (yawError < -180.0f)
        yawError += 360.0f;
    if (!usercmd.rcChannel[RC_LLR])
        Re->yaw = pidUpdate(&pidData[PID_ANGLE_YAW], yawError);
}

void attitudeResetAllPID(void)
{
    pidReset(&pidData[PID_ANGLE_ROLL]);
    pidReset(&pidData[PID_ANGLE_PITCH]);
    pidReset(&pidData[PID_ANGLE_YAW]);
    pidReset(&pidData[PID_RATE_ROLL]);
    pidReset(&pidData[PID_RATE_PITCH]);
    pidReset(&pidData[PID_RATE_YAW]);
}
